#ifndef CAMERA_NODE_HPP
#define CAMERA_NODE_HPP

#include <chrono>
#include <memory>
#include <thread>
#include <vector>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions/pcl_conversions.h>
#include "rclcpp/rclcpp.hpp"
#include "pcl_process.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include <mutex>

#include "base_interface/msg/cv_data.hpp"
#include"base_interface/msg/ops.hpp"
#include"base_interface/msg/mcu_comm.hpp"
#include "trajectory_prediction.hpp"

using namespace std::chrono_literals;
using std::vector;

typedef pcl::PointXYZ 				pointType;				
typedef pcl::PointCloud<pointType> 	pointCloud;				
typedef pointCloud::Ptr 			pPointCloud;

// base_interface::msg::Ops OpsMsg;//定位系统数据


class CvNode : public rclcpp::Node {
public:
	CvNode() : Node("CvNode") {
		pclTalker = this->create_publisher<sensor_msgs::msg::PointCloud2>("pcl_talker", 10);
		cvTalker = this->create_publisher<base_interface::msg::CvData>("Cv_Talker",10);
		//定位系统接收函数
		ops_sub_ = this->create_subscription<base_interface::msg::Ops>("/ops_data",10, std::bind(&CvNode::OpsCallback,this,std::placeholders::_1));
		//下位机的数据接收
		slave_sub_ = this->create_subscription<base_interface::msg::McuComm>("/slave_data",10,std::bind(&CvNode::SlaveCallback,this,std::placeholders::_1));
	}

	void OpsCallback(const base_interface::msg::Ops::ConstPtr msg)
	{

		OpsMsg_yaw = msg->theta; //车的偏航角
		OpsMsg_x    = msg->x    ; //车的x位置
		OpsMsg_y     = msg->y    ; //车的y位置
		// std::cout<<"x"<< OpsMsg.x <<"y"<< OpsMsg.y <<std::endl;
		// OpsMsg.x_vel = msg->x_vel; 
		// OpsMsg.y_vel = msg->y_vel; 
		// OpsMsg.x_acc = msg->x_acc;
		// OpsMsg.y_acc = msg->y_acc;


	}
	void SlaveCallback(base_interface::msg::McuComm::SharedPtr slave_msg)
	{
		table_yaw = slave_msg->num[8];//上层机构偏航位置
		// std::cout <<"table_yaw: "<<table_yaw<<std::endl;
	}

	void publishPcl(pPointCloud pcl_data) {
		auto message = sensor_msgs::msg::PointCloud2();
		pcl::toROSMsg(*pcl_data, message);
		message.header.stamp = this->now();
		message.header.frame_id = "camera";
		this->pclTalker->publish(message);

		RCLCPP_INFO(this->get_logger(), "I talk");
	}

	void publishCvData(OutputValue tra_output) {
		auto message = base_interface::msg::CvData();
		message.delta_time = tra_output.delta_time;
		message.out_time = tra_output.out_time;
		message.end_speed = tra_output.end_speed;
		message.end_x = tra_output.end_x;
		message.end_y = tra_output.end_z;
		message.end_yaw = tra_output.end_yaw;
		message.end_pitch = tra_output.end_pitch;
		message.max_z = -tra_output.max_y;
		message.end_speed_x = tra_output.end_speed_x;
		message.end_speed_y = tra_output.end_speed_z;
		message.end_speed_z = -tra_output.end_speed_y;
		message.current_x = tra_output.current_x;
		message.current_y = tra_output.current_z;
		message.current_z = -tra_output.current_y;
		message.current_speed_x = tra_output.current_speed_x;
		message.current_speed_y = tra_output.current_speed_z;
		message.current_speed_z = -tra_output.current_speed_y;
		message.current_yaw = tra_output.current_yaw;
		message.current_pitch = tra_output.current_pitch;
		// message.result_x_2 = tra_output.result_x_2;
		message.result_x_1 = tra_output.result_x_1;
		message.result_x_0 = tra_output.result_x_0;
		// message.result_y_2 = tra_output.result_z_2;
		message.result_y_1 = tra_output.result_z_1;
		message.result_y_0 = tra_output.result_z_0;
		message.result_z_2 = -tra_output.result_y_2;
		message.result_z_1 = -tra_output.result_y_1;
		message.result_z_0 = -tra_output.result_y_0;
		this->cvTalker->publish(message);
	}

public:
	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pclTalker;
	rclcpp::Publisher<base_interface::msg::CvData>::SharedPtr cvTalker;

	rclcpp::Subscription<base_interface::msg::Ops>::SharedPtr ops_sub_;
	rclcpp::Subscription<base_interface::msg::McuComm>::SharedPtr slave_sub_;



};

#endif